#include "app.h"

#include "command.h"

#include "motor_a_tim2_pwm.h"
#include "motor_b_tim4_pwm.h"

#include "motor_a_foc_config.h"
#include "motor_a_speed_config.h"
#include "motor_a_position_config.h"

#include "motor_b_foc_config.h"
#include "motor_b_speed_config.h"
#include "motor_b_position_config.h"

#include "current_calibration.h"
#include "encoder_calibration.h"

#include "delay.h"


/*
 * @brief   用户内容初始化
 *
 * @param   none
 *
 * @return  none 
 */
void APP_Init(void)
{
	/* MCU DRIVE INIT BEGIN */
	USART_DATA_Init();		// 串口DMA接收数据初始化
	
	MOTORA_TIM2_Init();		// 电机A定时器初始化
	MOTORB_TIM4_Init();		// 电机B定时器初始化
	
	STATIC_CURRENT_CALIBRATION(0);		// 静态电流校准
	MAGNETIC_ENCODER_CALIBRATION(0);	// 传感器校准
	/* MCU DRIVE INIT END*/
	
	
	/* PERIPHERAL INIT BEGIN */
	MOTORA_FOC_Config_Init();			// 电机A电流环初始化
	MOTORA_SPEED_Config_Init();			// 电机A速度环初始化
	MOTORA_POSITION_Config_Init();		// 电机A位置环初始化

 	MOTORB_FOC_Config_Init();			// 电机B电流环初始化
	MOTORB_SPEED_Config_Init();			// 电机B速度环初始化
	MOTORB_POSITION_Config_Init();		// 电机B位置环初始化
	/* PERIPHERAL INIT END*/
	
	
	/* FUNCTION INIT BEGIN */

	HAL_TIM_Base_Start_IT(&htim2);		// 电机A开始中断控制
	// HAL_TIM_Base_Start_IT(&htim4);	// 电机B开始中断控制
	
	/* FUNCTION INIT END*/
}

/**
 * @brief   用户内容循环
 *
 * @param   none
 *
 * @return  none
 */
uint8_t currentflag;
uint8_t speedflag;
uint8_t positionflag;
int16_t angle = 0;
void APP_Loop(void)
{
	/* MCU DRIVE INIT BEGIN */
	/* MCU DRIVE INIT END*/
	
	/* PERIPHERAL INIT BEGIN */
	// 1. 电流环 ---->>>> 2. 速度环 ---->>>> 3. 位置环
	// MOTORA_FOC_Config_Printf();			// 调试信息输出（如果需要调节电流环PID参数）
	// MOTORA_SPEED_Config_Printf();		// 调试信息输出（如果需要调节速度环PID参数）
	// MOTORA_POSITION_Config_Printf();		// 调试信息输出（如果需要调节位置环PID参数）
	
	USART_DATA_Proc((char*)rx_buffer);		// 串口缓冲区数据处理
	
	// 电流环调试(每2000ms更改一次目标电流)
	RUN_BY_LIMIT_BLOCK(2000,
		if (currentflag == 0)
		{
			MOTORA_SetTarIDIQ(0.0, 2.0);
			currentflag = 1;
		}
		else
		{
			MOTORA_SetTarIDIQ(0.0, 1.0);
			currentflag = 0;
		}
	)
	// 速度环调试(每4000ms更改一次目标速度)
//	RUN_BY_LIMIT_BLOCK(4000,
//		if (currentflag == 0)
//		{
//			MOTORA_SPEED_Config_SetTar(10);
//			currentflag = 1;
//		}
//		else
//		{
//			MOTORA_SPEED_Config_SetTar(5);
//			currentflag = 0;
//		}
//	)
	// 位置环调试(每2000ms更改一次目标位置)
//	RUN_BY_LIMIT_BLOCK(2000,
//		if (currentflag == 0)
//		{
//			MOTORA_POSITION_Config_SetTar(90);
//			currentflag = 1;
//		}
//		else
//		{
//			MOTORA_POSITION_Config_SetTar(60);
//			currentflag = 0;
//		}
//	)
	
//	MOTORA_SPEED_Config_Loop();		// 电机A速度环循环
//	MOTORA_POSITION_Config_Loop();	// 电机A位置环循环
//	
//	MOTORB_SPEED_Config_Loop();		// 电机B速度环循环
//	MOTORB_POSITION_Config_Loop();	// 电机B位置环循环
	/* PERIPHERAL INIT END*/
	
	/* FUNCTION INIT BEGIN */
	/* FUNCTION INIT END*/
}

